#include "myslam/transfer.h"
using namespace std;

namespace myslam {

Transfer::Transfer(){

    const char* uart = "/dev/ttyUSB0";
    // char* uart = "/dev/ttyS2";
    if((fd=open(uart,O_RDWR | O_NOCTTY))<0){
		printf("串口打开失败\n");
	}
    else{
		TransferInit(fd,460800,8,'N',1);

        transdata.transdata_struct.head1 = 0xf1;
        transdata.transdata_struct.head2 = 0xf2;
        transdata.transdata_struct.tail2 = 0xf2;
        transdata.transdata_struct.tail1 = 0xf1;

        transdata.transdata_struct.t[0] = 0;
        transdata.transdata_struct.t[1] = 0;
        transdata.transdata_struct.t[2] = 0;
        transdata.transdata_struct.valid_flag = 0;
        
        // sleep(1);
        transfer_running1_.store(true);
        transferReceloop_thread_ = std::thread(std::bind(&Transfer::TransferReceLoop,this));

        transfer_running2_.store(true);
        transferTranloop_thread_ = std::thread(std::bind(&Transfer::TransferTranLoop,this));
    }
}

Transfer::~Transfer(){
    TransferStop();
}


int Transfer::TransferInit(int fd,int nSpeed, int nBits, char nEvent, int nStop){
		struct termios newtio,oldtio;
		if  ( tcgetattr( fd,&oldtio)  !=  0) {     
			perror("SetupSerial 1");
			return -1;
		}
		bzero( &newtio, sizeof( newtio ) );
		newtio.c_cflag  |=  CLOCAL | CREAD;
		newtio.c_cflag &= ~CSIZE;
 
		switch( nBits )
		{
			case 7:
				newtio.c_cflag |= CS7;
				break;
			case 8:
				newtio.c_cflag |= CS8;
				break;

            default:
			    perror("com set error");
			    return -1;
            break;
		}
 
		switch( nEvent )
		{
            case 'O':
                newtio.c_cflag |= PARENB;
                newtio.c_cflag |= PARODD;
                newtio.c_iflag |= (INPCK | ISTRIP);
                break;
            case 'E': 
                newtio.c_iflag |= (INPCK | ISTRIP);
                newtio.c_cflag |= PARENB;
                newtio.c_cflag &= ~PARODD;
                break;
            case 'N':  
                newtio.c_cflag &= ~PARENB;
                break;

            default:
			    perror("com set error");
			    return -1;
            break; 
            
		}

		switch( nSpeed )
		{
			case 2400:
				cfsetispeed(&newtio, B2400);
				cfsetospeed(&newtio, B2400);
				break;
			case 4800:
				cfsetispeed(&newtio, B4800);
				cfsetospeed(&newtio, B4800);
				break;
			case 9600:
				cfsetispeed(&newtio, B9600);
				cfsetospeed(&newtio, B9600);
            break;
            case 115200:
				cfsetispeed(&newtio, B115200);
				cfsetospeed(&newtio, B115200);
            case 460800:
				cfsetispeed(&newtio, B460800);
				cfsetospeed(&newtio, B460800);
            break;
            default:
			    perror("com set error");
			    return -1;
            break;
		}

        if( nStop == 1 )
            newtio.c_cflag &= ~CSTOPB;
        else if ( nStop == 2 )
            newtio.c_cflag |= CSTOPB;

        // newtio.c_cc[VTIME] = 0;
        // newtio.c_cc[VMIN] = 0;

      	tcsetattr( fd,TCSANOW,&newtio);

		outfile.open("slam_data.txt",ios::trunc);
		printf("串口设置完成!\n\r");
		return 0;
}

void Transfer::TransferStop(){
    transfer_running1_.store(false);
    transferReceloop_thread_.join();

    transfer_running2_.store(false);
    transferTranloop_thread_.join();
    sleep(1);
    outfile.close();
    
}

void Transfer::TransferReceLoop(){ 
    uint8_t c = '\0';

    uint8_t data_cnt = 0;
    rece_status = INIT; 

    while(transfer_running1_.load()){
        while(read(fd,&c,1)<=0);
        switch(rece_status){
            case INIT:
                if(c==0xfd){
                    rece_status = HEAD1;
                    data_cnt =0;
                }
                else{
                    data_cnt = 0;
                    rece_status = INIT;
                    printf("fd\n\r");
                }
            break;

            case HEAD1:
                if(c==0xfe){
                    data_cnt =0;
                    rece_status = HEAD2;
                }
                else{
                    data_cnt = 0;
                    rece_status = INIT;
                    printf("fe\n\r");
                }
            break;

            case HEAD2:
                rece_status = DATA;
                recedata.data[data_cnt] = c;
                data_cnt++;
            break;

            case DATA:
                if(data_cnt<33){
                    // printf("data\n\r");
                    recedata.data[data_cnt] = c;
                    data_cnt++;
                }
                else{
                    uint8_t sum = 0;
                    for(uint8_t i=0;i<33;i++){
                        sum+=recedata.data[i];
                    }
                    sum+=0xfe;
                    sum+=0xfd;
                    if(c!=sum){
                        printf("sum\n\r");
                        data_cnt = 0;
                        rece_status = INIT;        
                    }
                    else{
                        rece_status = TAIL1;             
                    }
                }
            break;

            case TAIL1:
                if(c!=0xf2){
                    data_cnt = 0;
                    rece_status = INIT;
                    printf("f2\n\r");
                }
                else{
                    rece_status = TAIL2;
                }
            break;

            case TAIL2:
                if(c!=0xf1){
                    data_cnt = 0;
                    rece_status = INIT;
                    printf("f1\n\r");
                }
                else{

                    uint16_t rece_pic_index = recedata.recedata_struct.pic_index;
                    outfile << recedata.recedata_struct.imu_sample_time << "        "  
                                << rece_pic_index << "        " 
                                << recedata.recedata_struct.pic_sample_time << "        " 
                                << recedata.recedata_struct.GYRO[0] << "        "
                                << recedata.recedata_struct.GYRO[1] << "        "
                                << recedata.recedata_struct.GYRO[2] << "        "
                                << recedata.recedata_struct.ACC[0] << "        "
                                << recedata.recedata_struct.ACC[1] << "        "
                                << recedata.recedata_struct.ACC[2] << "        "
                                << recedata.recedata_struct.q[0] << "        "
                                << recedata.recedata_struct.q[1] << "        "
                                << recedata.recedata_struct.q[2] << "        "
                                << recedata.recedata_struct.q[3] << endl;
                    data_cnt = 0;
                    rece_status = INIT;
                    // printf("a\n\r");
                }
            break;

            default:
                data_cnt = 0;
                rece_status = INIT;
            break;
        }     
    }
}


void Transfer::Transfer_TransData(float t0,float t1,float t2,uint8_t valid){
    std::unique_lock<std::mutex> lock(data_mutex2_);
    transdata.transdata_struct.t[0] = t0;
    transdata.transdata_struct.t[1] = t1;
    transdata.transdata_struct.t[2] = t2;
    transdata.transdata_struct.valid_flag = valid;
    transfer_trans_.notify_one();
}


void Transfer::TransferTranLoop(){
    while(transfer_running2_.load()){
        std::unique_lock<std::mutex> lock(data_mutex2_);
        transfer_trans_.wait(lock);
        transdata.transdata_struct.sum = 0;
        for(int i=0;i<17;i++){
            transdata.transdata_struct.sum += transdata.data[i];
        }
        int n = write(fd,&transdata,18);
    }
}



}  // namespace myslam